import rclpy                        # ROS2 Python接口库
from rclpy.node import Node         # ROS2 节点类
from recognition_interface.srv import GetPedPos
from recognition_interface.msg import Rectangle
from cv_bridge import CvBridge
import cv2
import os

OUTPUT_DIR = os.environ.get("ROS_WS")+"/output/"

class GetPosServer(Node):
    def __init__(self, name):
        super().__init__(name)
        self.srv = self.create_service(GetPedPos, 'GetPedPos', self.get_pos_callback)
        self.cnt = 0
        self.bridge = CvBridge()
    
    def get_pos_callback(self, request:GetPedPos.Request, reponse:GetPedPos.Response):
        # 获取行人及其位置中心
        img = self.bridge.imgmsg_to_cv2(request.imgmsg)
        reponse.rectangles = self.get_pos_by_img(img)

        self.get_logger().info(f"request finished, returning the pedestrian number:{len(reponse.rectangles)}")
        return reponse
        
    def get_pos_by_img(self, image):
        '''
          识别图像中的人体, 返回其边框, 保存识别后的图片到指定路径
            parameters
                image: 要识别的图像
            return
                rectangles: 每个人的边框
        '''
        # 初始化HOG描述符
        hog = cv2.HOGDescriptor()
        hog.setSVMDetector(cv2.HOGDescriptor_getDefaultPeopleDetector())

        # 检测输入图像中的人类
        (humans, _) = hog.detectMultiScale(image, winStride=(3,3),
                                           padding=(1, 1), scale=1.1)
        rectangles = []
        for human in humans:
            rec = Rectangle()
            rec.side = human
            rectangles.append(rec)

        self.cnt+=1
        return rectangles

def main(args=None):
    rclpy.init(args=args)
    node = GetPosServer("GetPedPos_server")
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__=="__main__":
    main()